/*
packets.cpp
Description:    Functions for sending and receiving data packets
                according to the telemetry suit protocol (TN004A Suit Protocol)

Author:         T Bristow

Date Created:   20/04/13 
Date Modified:  02/06/13
Revision:       4
*/

#include "packets.h"

// Encodes packet structure and sends to controller using serial connection
void sendPacket(Packet* packet){
  Serial.write(packet->StartPacket);    // Write start packet byte (0x01)
  Serial.write(packet->MessageType);    // Write message type
  Serial.write((byte*)&(packet->DataLength),2);    // Write dataLength
  Serial.write((byte*)(packet->DataContent),packet->DataLength); // Write DataLength bytes from buffer pointed to by DataContent
  Serial.write((byte*)&(packet->DataChecksum),2); // Write checksum
  Serial.write(packet->EndPacket);    // Write End Paceket (0x04)
}

void updateChecksum(Packet* packet){ // Calculate checksum and store it in the packet structure
  packet->DataChecksum=calculateChecksum(packet);
}

unsigned short calculateChecksum(Packet* packet){ // Calculate checksum and return checksum value
  byte csumLow=0;    // low byte of checksum
  byte csumHigh=0;    // High byte of checksum
  unsigned short l;    // position in data buffer
  l=0;
  while(l<(packet->DataLength-1)){ // loop over data length
    csumLow^=packet->DataContent[l++];  // xor low byte with byte at l
    csumHigh^=packet->DataContent[l++]; // xor high byte with byte at l
  }
  if(l<packet->DataLength)    // check for an odd number of bytes
    csumLow^=packet->DataContent[l++];
  return (csumHigh<<8|csumLow);    // return 16bit checksum value
}

int readPacket(Packet* packet){ // Tries to read packet from data stream into packet structure, returns 1 if reads packet otherwise 0
  int result=0;
  while((Serial.available())&&(Serial.peek()!=PACKET_START)) // read from buffer until a packet start byte is available
    Serial.read();    // read byte from buffer and discard
  if(Serial.available()<=6) // If there are less than 6 bytes available in the buffer (min packet size) no packet available, return 0
    return 0;
  Serial.read(); // read start packet byte and discard
  packet->MessageType=Serial.read(); // read message type from the data stream
  packet->DataLength=Serial.read()|(Serial.read()<<8); // read data length bytes (low byte then high byte)
  if(packet->DataLength>=(Serial.available()-3)) // if there are data length bytes available+checksum bytes and end packet
  {
    for(int i=0;i<packet->DataLength;i++)
      packet->DataContent[i]=Serial.read();    // read data content into array
    packet->DataChecksum=Serial.read()|(Serial.read()<<8); // read checksum bytes
    packet->EndPacket=Serial.read(); // read end packet byte
    return 1;    // return 1 (successfully read packet)
  }
  return 0;
}